#define DEBUG         0 
#define FP_PRE        0.0000001
#define USAGE_MESSAGE "Usage: llsellipsoid <file>"
#define DATA_POINTS   4
#define EVIDENCE_TRSH 500
#define CENTROID_TRSH_HIGH 100
#define CENTROID_TRSH_LOW 20
#define MAX_CENTROIDS 500

#include <iostream>
using std::cout;
using std::cerr;
using std::endl;

#include <stdio.h>
#include <stdlib.h>
#include <cstdlib>
#include <string.h>
#include <time.h>
#include <math.h>
#include <cmath>
#include <fstream>
using std::ifstream;
using std::ofstream;

#include "util.h"

typedef struct {
	double x;
	double y;
	double z;
} edge_pixel_t, point_3d_t, plane_coeff;

typedef struct {
	edge_pixel_t point;
	double distance;
} pixel_distance;

typedef struct {
	edge_pixel_t point;
	int centroidIndex;
} point_list;

typedef struct {
	point_3d_t centroid;
	point_3d_t meanCentroid;
	int votes;
} centroid_list;

typedef struct {
	double x;
	double y;
	double z;
	edge_pixel_t p1;
	edge_pixel_t p2;
	edge_pixel_t p3;
	edge_pixel_t p4;
} pixel_centroid;

typedef struct {
	int votes;
	double a;
	double b;
	double c;
	double alpha;
	double beta;
	double theta;
} accum_element_axis_angles;

typedef struct {
	matrix_t_3D <int> centroid;
	int centroid_thresh;
	matrix_t_3D <accum_element_axis_angles> axis_angles;
} accumulator_t;

typedef struct {
	double x;
	double y;
	double z;
	double a;
	double b;
	double c;
	double alpha;
	double beta;
	double theta;
} ellipsoid_t;

string        in_filename        ;
string        out_filename       ;
edge_pixel_t* edge_pxls    = NULL;
int           edge_pxl_cnt = 0   ;
point_list* voters = NULL;
int 		  numVoters 	= 0;
centroid_list* possCentroids = NULL;
int 		  numCentroids 	= 0;
int			  width        = 500 ;
int			  height       = 500 ;
int			  depth        = 500 ;
int			  count		   = 0;
int			  centroidCount= 0;
accumulator_t accum        = {{0, 0, 0, NULL}, 0,{0, 0, 0, NULL}};
matrix_t <double>      X            		 = {0, 0, NULL};
matrix_t <double>      Xt           		 = {0, 0, NULL};
matrix_t <double>      XtX          		 = {0, 0, NULL};
matrix_t <double>      Y           		     = {0, 0, NULL};
matrix_t <double>      Xty          		 = {0, 0, NULL};
matrix_t <double>	   aug      			 = {0, 0, NULL};
matrix_t <double>	   aug2      			 = {0, 0, NULL};
matrix_t <double>	   augmented  			 = {0, 0, NULL};
matrix_t <point_3d_t>  intersectionLines    = {0, 0, NULL};

void setup(int*, char**);
void setup_accum();
//void setup_lls();
void terminate();
//void shutdown_lls();
void shutdown_accum();
void gather_evidence();
edge_pixel_t* random_points(int);
void findTangentPlanes(plane_coeff *, int *);
void fitTangentPlane(plane_coeff &, int);
void findIntersection(plane_coeff *, int *);
void findIntersectionPoint(plane_coeff, plane_coeff, point_3d_t &, point_3d_t &);
void process_accum();
void hessianNormalForm(plane_coeff, plane_coeff &, double &);
void calcX0(plane_coeff, plane_coeff, double, double, point_3d_t &);
void directionFromNullSpace(plane_coeff, plane_coeff, point_3d_t &);
void gauss(matrix_t <double>* , double*);
bool coPlanar(int *);
void findNeighborhood(edge_pixel_t, pixel_distance *);
double calcDistance(edge_pixel_t, edge_pixel_t);
void findCentroid(plane_coeff *, int *, int);
void getLineMidpointPlanes(point_3d_t, plane_coeff &, int);
void findMidPoint(point_3d_t, point_3d_t, point_3d_t &);
void findAxisLengthsAndOrientation();
void fitPoints6D(int *, double &, double &, double &, double &, double &, double &);
void calcEigenvalues(double, double, double, double, double, double, double &, double &, double &);
void insertionSort(pixel_distance [],int);
void insertionSortCentroids(point_3d_t [],int);
void crossProduct(point_3d_t, point_3d_t, point_3d_t &);
double dotProduct(point_3d_t, point_3d_t);

//void findIntersectionPoint(plane_coeff, plane_coeff,point_3d_t&,point_3d_t&,point_3d_t*,point_3d_t*);

int main (int argc, char *argv[]) {
	ellipsoid_t ellipsoid;
	timespec start, end, t_ge, t_pa;
	//printf("Wait a minute...\n");
		hr_timer_t rng_time;
	// setup the program
	setup(&argc,argv);
	
	printf("Setup DONE.\n");

	timer_start(&rng_time);
	gather_evidence();
	timer_end(&rng_time);
	
	printf("run time: %f\n", compute_secs(&rng_time.elapsed));

	//~ // recover the circle from the evidence
	//~ clock_gettime(CLOCK_PROCESS_CPUTIME_ID,&start);
	//~ process_accum(&ellipsoid);
	//~ clock_gettime(CLOCK_PROCESS_CPUTIME_ID,&end);
	//~ time_diff(&start,&end,&t_pa);
	//~ 
	//cout << "x.center: " << ellipsoid.x << endl;
	//cout << "y.center: " << ellipsoid.y << endl;
	//cout << "z.center: " << ellipsoid.z << endl;
	//~ cout << "a:        " << ellipsoid.a << endl;
	//~ cout << "b:        " << ellipsoid.b << endl;
	//~ cout << "c:        " << ellipsoid.c << endl;
	//~ cout << "alpha:    " << ellipsoid.alpha << endl;
	//~ cout << "beta:     " << ellipsoid.beta << endl;
	//~ cout << "theta:    " << ellipsoid.theta << endl;
//~ 
	//~ out_filename = in_filename + "-output";

	//edge_pixel_t * edgePixels = new edge_pixel_t[edge_pxl_cnt];

	//~ FILE* file;
//~ 
	//~ file = fopen(out_filename.c_str(), "w");
	//~ fprintf(file, "9\n");
	//~ fprintf(file,"%f %f %f %f %f %f %f %f %f\n", ellipsoid.x, ellipsoid.y, ellipsoid.z,
								  //~ ellipsoid.a, ellipsoid.b, ellipsoid.c,
								  //~ ellipsoid.alpha, ellipsoid.beta, ellipsoid.theta);
//~ 
	//~ for(double azimuth = 0.0; azimuth < 180.0; azimuth += 0.5)
	//~ {
			//~ for(double polar = 0.0; polar < 90.0; polar += 0.5)
			//~ {
					//~ edge_pixel_t tempPt;
					//~ double tempX, tempY, tempZ;
					//~ double alpha = ellipsoid.alpha;
					//~ double beta = ellipsoid.beta;
					//~ double theta = ellipsoid.theta;
					//~ 
					//~ tempX = ellipsoid.a * cos(azimuth) * sin(polar);
					//~ tempY = ellipsoid.b * sin(azimuth) * sin(polar);
					//~ tempZ = ellipsoid.c * cos(polar);
					//~ 
					//~ tempPt.x = cos(beta)*cos(theta)*tempX + (cos(alpha)*sin(theta)+sin(alpha)*sin(beta)*cos(theta))*tempY +
								//~ (sin(alpha)*sin(theta)-cos(alpha)*sin(beta)*cos(theta))*tempZ;
					//~ tempPt.y = (-cos(beta))*sin(theta)*tempX + (cos(alpha)*cos(theta)-sin(alpha)*sin(beta)*sin(theta))*tempY +
								//~ (sin(alpha)*cos(theta)+cos(alpha)*sin(beta)*sin(theta))*tempZ;
					//~ tempPt.z = sin(beta)*tempX + (-sin(alpha))*cos(beta)*tempY + cos(alpha)*cos(beta)*tempZ;
//~ 
					//~ tempPt.x += centerX;
					//~ tempPt.y += centerY;
					//~ tempPt.z += centerZ;
					//~ 
					//~ edgePixels[numPixels] = tempPt;
					//~ 
					//~ numPixels++;
					//~ 
//~ 
					//~ //fprintf(file,"%f %f %f\n", x,y,z);
			//~ }
	//~ }
	//~ 
	//~ fprintf(file,"%d\n", numPixels);
	//~ 
	//~ for(int i = 0; i < edge_pxl_cnt; i++)
	//~ {
		//~ fprintf(file,"%f %f %f\n", edgePixels[i].x,edgePixels[i].y,edgePixels[i].z);
	//~ }
//~ 
	//~ fclose(file);
	
	
	terminate();
	return 0;
}

void setup(int* argc, char **argv) {
	
	//printf("Whoa.\n");
	int pos;

	// check the lenght of the cmd line
	if (*argc != 2)
		cerr << USAGE_MESSAGE << endl;

	// seed the rng
	srand(clock());

	// load the input image
	debug("Reading in file %s...",argv[1]);
	in_filename = argv[1];
	
	//cout << in_filename << endl;
	//printf("file name: %s\n", argv[1]);
	
	ifstream inFile(in_filename.c_str());
	double temp;
	
	inFile >> temp >> temp >> temp >> temp >> temp >> temp >> temp >> temp >> temp >> temp >> edge_pxl_cnt;
	
	//cout << edge_pxl_cnt << endl;
	
	edge_pxls = new edge_pixel_t[edge_pxl_cnt];
	memset(edge_pxls,0,sizeof(edge_pixel_t) * edge_pxl_cnt);
	
	//printf("Before for.\n");
	
	for(int i = 0; i < edge_pxl_cnt; i++)
	{
		inFile >> temp;
		edge_pxls[i].x = temp;

		inFile >> temp;
		edge_pxls[i].y = temp;

		inFile >> temp;
		edge_pxls[i].z = temp;
	}
	//printf("After for.\n");

	// setup the accumulator
	setup_accum();

}

/*void setup_lls() {
	matrix_allocate(&aug, 6          , 5);
	matrix_allocate(&X  , DATA_POINTS, 4);
	matrix_allocate(&Xt , 4          , DATA_POINTS);
	matrix_allocate(&XtX, 4          , 4);
	matrix_allocate(&y  , DATA_POINTS, 1);
	matrix_allocate(&Xty, 4          , 1);
}*/

void setup_accum() {
	// allocate the accumulator for the ellipse center
	accum.centroid_thresh = 0;
	printf("Before setup accum.\n");
	matrix_allocate_3D(&accum.centroid,width,height, depth);
	
	numVoters = DATA_POINTS*EVIDENCE_TRSH;
	voters = new point_list[numVoters];
	memset(voters,0,sizeof(point_list) * numVoters);
	
	numCentroids = MAX_CENTROIDS;
	possCentroids = new centroid_list[numCentroids];
	memset(possCentroids,0,sizeof(centroid_list) * numCentroids);
	printf("After setup accum.\n");
	//matrix_allocate(&accum.center_z_radius, depth, depth/2);
}

void terminate() {
	if (edge_pxls != NULL) {
		delete [] edge_pxls;
		edge_pxls    = NULL;
		edge_pxl_cnt = 0;
	}

	//shutdown_accum();
}

/*void shutdown_lls() {
	matrix_teardown(Xty);
	matrix_teardown(y);
	matrix_teardown(XtX);
	matrix_teardown(Xt);
	matrix_teardown(X);
	matrix_teardown(aug);
}*/

void shutdown_accum() {
	//matrix_teardown(accum.center_x_y);
	//matrix_teardown(accum.center_z_radius);
}

void gather_evidence() {
	int evidence, *pxls;
	//double A, B, C, D, E;
	ellipsoid_t ellipsoid;
	plane_coeff * planes = new plane_coeff[4];

	// allocate the pixels storage
	pxls = new int[DATA_POINTS];

	evidence = 0;
	while (evidence < EVIDENCE_TRSH && accum.centroid_thresh < CENTROID_TRSH_HIGH)
	{
		memset(&ellipsoid,0,sizeof(ellipsoid_t));

		// randomly select DATA_POINTS # of edge pixels
		for (int i=0; i < DATA_POINTS; i++)
		{
			pxls[i] = random_int(0,edge_pxl_cnt);
		}
		
		if(!coPlanar(pxls))
		{
			
			findTangentPlanes(planes, pxls);
			
			findIntersection(planes, pxls);
			
			findCentroid(planes, pxls, evidence);
		}
		evidence++;
	}
	
	process_accum();
	
	printf("Iterations: %d\n", evidence);
	printf("Centroids\n");
	for(int temp = 0; temp < evidence; temp++)
	{
		if(possCentroids[temp].votes >= CENTROID_TRSH_LOW)
		{
			printf("%d: \t %f, %f, %f \t Votes: %d \n", temp, possCentroids[temp].centroid.x, possCentroids[temp].centroid.y, possCentroids[temp].centroid.z, possCentroids[temp].votes);
			printf("\t %f, %f, %f \n\n", possCentroids[temp].meanCentroid.x, possCentroids[temp].meanCentroid.y, possCentroids[temp].meanCentroid.z);
		}
	}
}

void findTangentPlanes(plane_coeff * planes, int * pxls)
{
	for(int i = 0; i < DATA_POINTS; i++)
	{
		fitTangentPlane(planes[i], pxls[i]);
	}
	for(int i = 0; i < DATA_POINTS; i++)
	{
		//~ printf("Point: %f, %f, %f\n", edge_pxls[pxls[i]].x, edge_pxls[pxls[i]].y, edge_pxls[pxls[i]].z);
		//~ printf("Tangent Plane Coefficients:\n");
		//~ printf("\tA: %f\n", planes[i].x);
		//~ printf("\tB: %f\n", planes[i].y);
		//~ printf("\tC: %f\n\n", planes[i].z);
	}
}

void fitTangentPlane(plane_coeff &plane, int pxl)
{
	int i,j;
	double *results;
	pixel_distance *closest = new pixel_distance[7];
	double sum_xx = 0.0, sum_yy = 0.0, sum_xy = 0.0,
		   sum_x = 0.0, sum_y = 0.0, sum_xz = 0.0, sum_yz = 0.0, sum_z = 0.0;
	
	// allocate
	matrix_allocate(&aug, 3, 4);
	
	results = new double[3];
	memset(results,0,sizeof(double) * 3);
	
	findNeighborhood(edge_pxls[pxl], closest);
	
	for(int i = 0; i < 6; i++)
	{
		sum_xx += closest[i].point.x * closest[i].point.x;
		sum_yy += closest[i].point.y * closest[i].point.y;
		sum_xy += closest[i].point.x * closest[i].point.y;
		sum_xz += closest[i].point.x * closest[i].point.z;
		sum_yz += closest[i].point.y * closest[i].point.z;
		sum_x  += closest[i].point.x;
		sum_y  += closest[i].point.y;
		sum_z  += closest[i].point.z;
	}
	
	aug.cells[0][0] = sum_xx;
	aug.cells[0][1] = sum_xy;
	aug.cells[1][0] = sum_xy;
	aug.cells[0][2] = sum_x;
	aug.cells[2][0] = sum_x;
	aug.cells[1][1] = sum_yy;
	aug.cells[1][2] = sum_y;
	aug.cells[2][1] = sum_y;
	aug.cells[2][2] = 6;
	aug.cells[0][3] = sum_xz;
	aug.cells[1][3] = sum_yz;
	aug.cells[2][3] = sum_z;

	// run a gaussian solver on the system of equations
	//~ printf("==========================================\n");
	//~ matrix_print("Augmented (pre-gauss)",&aug);
	gauss(&aug,results);
	//~ matrix_print("Augmented (post-gauss)",&aug);
	//~ printf("==========================================\n");

	// extract the results
	plane.x = results[0];
	plane.y = results[1];
	plane.z = results[2];
	
	//~ printf("A: %f\n", plane.x);
	//~ printf("B: %f\n", plane.y);
	//~ printf("C: %f\n\n", plane.z);

	// cleanup
	delete [] results;

}

void findIntersection(plane_coeff * planes, int * pxls)
{
	matrix_allocate(&intersectionLines, 6, 2);
	
	findIntersectionPoint(planes[0], planes[1], intersectionLines.cells[0][0], intersectionLines.cells[0][1]);
	findIntersectionPoint(planes[0], planes[2], intersectionLines.cells[1][0], intersectionLines.cells[1][1]);
	findIntersectionPoint(planes[0], planes[3], intersectionLines.cells[2][0], intersectionLines.cells[2][1]);
	findIntersectionPoint(planes[1], planes[2], intersectionLines.cells[3][0], intersectionLines.cells[3][1]);
	findIntersectionPoint(planes[1], planes[3], intersectionLines.cells[4][0], intersectionLines.cells[4][1]);
	findIntersectionPoint(planes[2], planes[3], intersectionLines.cells[5][0], intersectionLines.cells[5][1]);
	
}

void findIntersectionPoint(plane_coeff plane1, plane_coeff plane2, point_3d_t &interPoint1, point_3d_t &interPoint2)
{
	plane_coeff hessian1, hessian2;
	double P1, P2;
	point_3d_t X0, direction_X0;
	
	hessianNormalForm(plane1, hessian1, P1);
	hessianNormalForm(plane2, hessian2, P2);
	
	calcX0(hessian1, hessian2, P1, P2, X0);
	directionFromNullSpace(hessian1, hessian2, direction_X0);
	
	interPoint1.x = X0.x;
	interPoint1.y = X0.y;
	interPoint1.z = X0.z;
	
	
	interPoint2.x = direction_X0.x;
	interPoint2.y = direction_X0.y;
	interPoint2.z = direction_X0.z;
    
    //~ printf("Tangent Plane 1 Coefficients:\n");
	//~ printf("\tA: %f\n", plane1.x);
	//~ printf("\tB: %f\n", plane1.y);
	//~ printf("\tC: %f\n\n", plane1.z);  
	  //~ 
    //~ printf("Hessian Plane 1 Coefficients:\n");
	//~ printf("\tA: %f\n", hessian1.x);
	//~ printf("\tB: %f\n", hessian1.y);
	//~ printf("\tC: %f\n\n", hessian1.z);
	//~ 
	//~ printf("Hessian Plane 1 P Value: %f\n\n", P1);
//~ 
	//~ printf("Tangent Plane 2 Coefficients:\n");
	//~ printf("\tA: %f\n", plane2.x);
	//~ printf("\tB: %f\n", plane2.y);
	//~ printf("\tC: %f\n\n", plane2.z);
	  //~ 
    //~ printf("Hessian Plane 2 Coefficients:\n");
	//~ printf("\tA: %f\n", hessian2.x);
	//~ printf("\tB: %f\n", hessian2.y);
	//~ printf("\tC: %f\n\n", hessian2.z);
	//~ 
	//~ printf("Hessian Plane 2 P Value: %f\n\n", P2);
	
	//~ printf("Tangent Plane 3 Coefficients:\n");
	//~ printf("\tA: %f\n", plane3.x);
	//~ printf("\tB: %f\n", plane3.y);
	//~ printf("\tC: %f\n\n", plane3.z);
    
    //~ printf("X0 vector:\n");
    //~ printf("x: %f\n", X0.x);
    //~ printf("y: %f\n", X0.y);
    //~ printf("z: %f\n\n", X0.z);
    //~ 
    //~ printf("Direction vector:\n");
    //~ printf("x: %f\n", direction_X0.x);
    //~ printf("y: %f\n", direction_X0.y);
    //~ printf("z: %f\n\n", direction_X0.z);
    
    //~ printf("Intersection Point 1: %f, %f, %f\n\n", interPoint1.x, interPoint1.y, interPoint1.z);
    //~ printf("Intersection Point 2: %f, %f, %f\n\n", interPoint2.x, interPoint2.y, interPoint2.z);
    //~ printf("----------------------------------------------------------------------\n");
    
    //~ delete [] results;
}

void hessianNormalForm(plane_coeff plane, plane_coeff & hessian, double & P)
{
	hessian.x = plane.x / (sqrt(plane.x*plane.x + plane.y*plane.y + 1));
	hessian.y = plane.y / (sqrt(plane.x*plane.x + plane.y*plane.y + 1));
	hessian.z = -1 / (sqrt(plane.x*plane.x + plane.y*plane.y + 1));
	P = plane.z / (sqrt(plane.x*plane.x + plane.y*plane.y + 1));
}

void calcX0(plane_coeff hess1, plane_coeff hess2, double P1, double P2, point_3d_t & X0)
{
	matrix_allocate(&aug, 3, 4);
	
	double * results;
	results = new double[3];
	memset(results,0,sizeof(double) * 3);
	
	aug.cells[0][0] = hess1.x;
	aug.cells[0][1] = hess1.y;
	aug.cells[0][2] = hess1.z;
	aug.cells[0][3] = -P1;
	aug.cells[1][0] = hess2.x;
	aug.cells[1][1] = hess2.y;
	aug.cells[1][2] = hess2.z;
	aug.cells[1][3] = -P2;
	aug.cells[2][0] = 0;
	aug.cells[2][1] = 0;
	aug.cells[2][2] = 1;
	aug.cells[2][3] = hess1.z;
	
	//~ printf("==========================================\n");
	//~ matrix_print("Augmented (pre-gauss)",&aug);
	gauss(&aug,results);
	//~ matrix_print("Augmented (post-gauss)",&aug);
	//~ printf("==========================================\n");
	
	//~ matrix_allocate(&aug2, 3, 4);
	//~ 
	//~ for (int i=0; i < aug.Y_SIZE; i++) {
		//~ for(int j=0; j < aug.X_SIZE; j++)
			//~ aug2.cells[i][j] = aug.cells[i][j];
	//~ }
	//~ 
	//~ aug2.cells[2][0] = 0;
	//~ aug2.cells[2][1] = 0;
	//~ aug2.cells[2][2] = 1;
	//~ aug2.cells[2][3] = hess1.z;
	//~ 
	//~ results = new double[3];
	//~ memset(results,0,sizeof(double) * 3);
	
	//~ printf("==========================================\n");
	//~ matrix_print("Augmented (pre-gauss)",&aug2);
	//~ gauss(&aug2,results);
	//~ matrix_print("Augmented (post-gauss)",&aug2);
	//~ printf("==========================================\n");
	
	X0.x = results[0];
	X0.y = results[1];
	X0.z = results[2];
	
	delete [] results;
}

void directionFromNullSpace(plane_coeff hess1, plane_coeff hess2, point_3d_t & dir)
{
	matrix_allocate(&aug, 3, 4);
	
	double * results;
	results = new double[3];
	memset(results,0,sizeof(double) * 3);
	
	aug.cells[0][0] = hess1.x;
	aug.cells[0][1] = hess1.y;
	aug.cells[0][2] = hess1.z;
	aug.cells[0][3] = 0;
	aug.cells[1][0] = hess2.x;
	aug.cells[1][1] = hess2.y;
	aug.cells[1][2] = hess2.z;
	aug.cells[1][3] = 0;
	aug.cells[2][0] = 0;
	aug.cells[2][1] = 0;
	aug.cells[2][2] = 1;
	aug.cells[2][3] = hess1.z;
	
	//~ matrix_allocate(&aug2, 3, 4);
	//~ 
	//~ for (int i=0; i < aug.Y_SIZE; i++) {
		//~ for(int j=0; j < aug.X_SIZE; j++)
			//~ aug2.cells[i][j] = aug.cells[i][j];
	//~ }
	//~ 
	//~ aug2.cells[2][0] = 0;
	//~ aug2.cells[2][1] = 0;
	//~ aug2.cells[2][2] = 1;
	//~ aug2.cells[2][3] = hess1.z;
	//~ 
	//~ results = new double[3];
	//~ memset(results,0,sizeof(double) * 3);
	
	//~ printf("==========================================\n");
	//~ matrix_print("Augmented (pre-gauss)",&aug2);
	//~ gauss(&aug2,results);
	//~ matrix_print("Augmented (post-gauss)",&aug2);
	//~ printf("==========================================\n");
	
	gauss(&aug,results);
	
	dir.x = results[0];
	dir.y = results[1];
	dir.z = results[2];
	
	delete [] results;
}

// implemented from psudocode at: http://en.wikipedia.org/wiki/Gaussian_elimination#Pseudocode
// gaussian elimination with partial pivoting
// results is ordered x1,x2,x3...xn
void gauss(matrix_t <double>* A, double* results) {
	int i,j,k,u,maxi;
	double var, *tp;

	i = 0;
	j = 0;
	while (i < A->Y_SIZE && j < A->X_SIZE) {
		// find the row with the maximum value
		maxi = i;
		for (k=i+1; k < A->Y_SIZE; k++) {
			if (fabs(A->cells[k][j]) > fabs(A->cells[maxi][j]))
				maxi = k;
		}

		if (A->cells[maxi][j] != 0) {
			// swap rows
			if (i != maxi) {
				tp = A->cells[i];
				A->cells[i] = A->cells[maxi];
				A->cells[maxi] = tp;
			}

			// reduce pivot element to 1
			var = A->cells[i][j];
			for (k=0; k < A->X_SIZE; k++)
				A->cells[i][k] /= var;

			// remove the pivot element from all subsequent rows
			for (u=i+1; u < A->Y_SIZE; u++) {
				var = A->cells[u][j];
				for (k=j; k < A->X_SIZE; k++)
					A->cells[u][k] -= A->cells[i][k] * var;
			}

			i++;
		}

		j++;
	}

	// retrieve the results
	for (i=A->Y_SIZE-1; i >= 0; i--) {
		var = A->cells[i][A->X_SIZE-1];
		for (j=A->X_SIZE-2; j > i; j--) {
			var -= A->cells[i][j] * results[j];
		}
		results[i] = var;
		//printf("var: %f\n", var);
	}
	//printf("\n");

}

void process_accum() {
	
	double distance = 0.0;
	point_3d_t centroidPt, currCentroidPt;
	
	for(int index = 0; index < MAX_CENTROIDS; index++)
	{
		for(int i = 0; i < MAX_CENTROIDS; i++)
		{
			//if(i != index)
			{
				distance = calcDistance(possCentroids[index].centroid, possCentroids[i].centroid);
				
				if(distance <= 5.0)
				{
					possCentroids[index].meanCentroid.x = ((possCentroids[index].meanCentroid.x + possCentroids[i].centroid.x) / 2.0);
					possCentroids[index].meanCentroid.y = ((possCentroids[index].meanCentroid.y + possCentroids[i].centroid.y) / 2.0);
					possCentroids[index].meanCentroid.z = ((possCentroids[index].meanCentroid.z + possCentroids[i].centroid.z) / 2.0);
					possCentroids[index].votes++;
				}
			}
		}
	}
//~ 
	//~ find_max_2d(&accum.center_x_y,max_i,max_j);
	//~ s->y = max_i;
	//~ s->x = max_j;
//~ 
	//~ find_max_2d(&accum.center_z_radius,max_i,max_j);
	//~ s->z = max_j;
	//~ s->radius = max_i; 
	
	//~ findMaxCentroids(&accum.centroid, possCentroids);
	
	
	//~ cout << accum.centroid.cells[max_i][max_j][max_k-1].votes << endl;
	//cout << accum.centroid.cells[max_i][max_j][max_k].votes << endl;
	//~ cout << accum.centroid.cells[max_i][max_j][max_k+1].votes << endl;
	
	//point_3d_t meanCentroid;
	
	//~ findMeanCentroid(accum.centroid.cells[max_i][max_j][max_k], meanCentroid);
	
	//~ s->x = meanCentroid.x;
	//~ s->y = meanCentroid.y;
	//~ s->z = meanCentroid.z;

}

bool coPlanar(int *pixels)
{
	edge_pixel_t pt31, pt21, pt43, crossProd;
	
	pt31.x = edge_pxls[pixels[2]].x - edge_pxls[pixels[0]].x;
	pt31.y = edge_pxls[pixels[2]].y - edge_pxls[pixels[0]].y;
	pt31.z = edge_pxls[pixels[2]].z - edge_pxls[pixels[0]].z;
	
	if(pt31.x == 0.0 && pt31.y == 0.0 && pt31.z == 0.0)
		return true;
	
	pt21.x = edge_pxls[pixels[1]].x - edge_pxls[pixels[0]].x;
	pt21.y = edge_pxls[pixels[1]].y - edge_pxls[pixels[0]].y;
	pt21.z = edge_pxls[pixels[1]].z - edge_pxls[pixels[0]].z;
	
	pt43.x = edge_pxls[pixels[3]].x - edge_pxls[pixels[2]].x;
	pt43.y = edge_pxls[pixels[3]].y - edge_pxls[pixels[2]].y;
	pt43.z = edge_pxls[pixels[3]].z - edge_pxls[pixels[2]].z;
	
	crossProd.x = pt21.y*pt43.z - pt43.y*pt21.z;
	crossProd.y = pt21.z*pt43.x - pt43.z*pt21.x;
	crossProd.z = pt21.x*pt43.y - pt43.x*pt21.y;
	
	if((pt31.x*crossProd.x + pt31.y*crossProd.y + pt31.z*crossProd.z) == 0.0)
		return true;
		
	return false;
}

void findNeighborhood(edge_pixel_t currPoint, pixel_distance *closest)
{
	int i = 0;
	//~ printf("Closest: %f, %f, %f\n",
		//~ currPoint.x, currPoint.y, currPoint.z);
	for(i = 0; i < 6; i++)
	{
		closest[i].point = edge_pxls[i];
		closest[i].distance = calcDistance(currPoint, closest[i].point);
		//printf("Current: %f, %f, %f; Chosen:  %f, %f, %f; Dist: %f\n",
		//		currPoint.x, currPoint.y, currPoint.z,
		//		closest[6].point.x, closest[6].point.y, closest[6].point.z,
		//		closest[6].distance);		
		//printf("dist: %f\n", closest[i].distance);
	}
	
	//~ for(int j = 0; j < 7; j++)
	//~ {
		//~ printf("Closest: %f, %f, %f; Dist: %f\n",
			//~ closest[j].point.x, closest[j].point.y, closest[j].point.z,
			//~ closest[j].distance);
	//~ }
	//~ printf("\n");
	
	while(i < edge_pxl_cnt)
	{
		
		closest[6].point = edge_pxls[i];
		closest[6].distance = calcDistance(currPoint, closest[6].point);
		//printf("Current: %f, %f, %f; Chosen:  %f, %f, %f; Dist: %f\n",
		//		currPoint.x, currPoint.y, currPoint.z,
		//		closest[6].point.x, closest[6].point.y, closest[6].point.z,
		//		closest[6].distance);
		//printf("dist: %f\n", closest[6].distance);		
		insertionSort(closest, 7);
		
		//~ for(int j = 0; j < 7; j++)
		//~ {
			//~ printf("Closest: %f, %f, %f; Dist: %f\n",
				//~ closest[j].point.x, closest[j].point.y, closest[j].point.z,
				//~ closest[j].distance);
		//~ }
		//~ printf("\n");
		
		//printf("i: %d\n", i);
		i++;
		
	}
}

double calcDistance(edge_pixel_t p1, edge_pixel_t p2)
{
	return sqrt((p1.x - p2.x)*(p1.x - p2.x) + (p1.y - p2.y)*(p1.y - p2.y) + (p1.z - p2.z)*(p1.z - p2.z));
}

void findCentroid(plane_coeff * planes, int * pxls, int index)
{
	point_3d_t * midPoints = new point_3d_t[4];
	plane_coeff * lineMidpointPlanes = new plane_coeff[4];
	//point_3d_t * c_vecs = new point_3d_t[DATA_POINTS];
	//point_3d_t * pointsOnInterLines = new point_3d_t[6];
	point_3d_t tempPt1, tempPt2, P_L1, P_L2, centroidPt, p1, p2;
	double d, u;
	
	findMidPoint(edge_pxls[pxls[0]], edge_pxls[pxls[1]], midPoints[0]);
	findMidPoint(edge_pxls[pxls[0]], edge_pxls[pxls[2]], midPoints[1]);
	findMidPoint(edge_pxls[pxls[0]], edge_pxls[pxls[3]], midPoints[2]);
	findMidPoint(edge_pxls[pxls[1]], edge_pxls[pxls[2]], midPoints[3]);
	//~ findMidPoint(edge_pxls[pxls[1]], edge_pxls[pxls[3]], midPoints[4]);
	//~ findMidPoint(edge_pxls[pxls[2]], edge_pxls[pxls[3]], midPoints[2]);
	
	//~ printf("Midpoint 12:\n");
    //~ printf("x: %f\n", midPoints[0].x);
    //~ printf("y: %f\n", midPoints[0].y);
    //~ printf("z: %f\n\n", midPoints[0].z);
    //~ 
    //~ printf("Point 1:\n");
    //~ printf("x: %f\n", edge_pxls[pxls[0]].x);
    //~ printf("y: %f\n", edge_pxls[pxls[0]].y);
    //~ printf("z: %f\n\n", edge_pxls[pxls[0]].z);
    //~ printf("Midpoint 13:\n");
    //~ printf("x: %f\n", midPoints[1].x);
    //~ printf("y: %f\n", midPoints[1].y);
    //~ printf("z: %f\n\n", midPoints[1].z);
    //~ printf("Point 3:\n");
    //~ printf("x: %f\n", edge_pxls[pxls[2]].x);
    //~ printf("y: %f\n", edge_pxls[pxls[2]].y);
    //~ printf("z: %f\n\n", edge_pxls[pxls[2]].z);
    //~ 
    //~ printf("Midpoint 23:\n");
    //~ printf("x: %f\n", midPoints[2].x);
    //~ printf("y: %f\n", midPoints[2].y);
    //~ printf("z: %f\n\n", midPoints[2].z);
	
	getLineMidpointPlanes(midPoints[0], lineMidpointPlanes[0], 0);
	getLineMidpointPlanes(midPoints[1], lineMidpointPlanes[1], 1);
	getLineMidpointPlanes(midPoints[2], lineMidpointPlanes[2], 2);
	getLineMidpointPlanes(midPoints[3], lineMidpointPlanes[3], 3);
	
	//~ printf("Plane 12 Coefficients:\n");
	//~ printf("\tA: %f\n", lineMidpointPlanes[0].x);
	//~ printf("\tB: %f\n", lineMidpointPlanes[0].y);
	//~ printf("\tC: %f\n\n", lineMidpointPlanes[0].z);
	//~ 
	//~ printf("Plane 13 Coefficients:\n");
	//~ printf("\tA: %f\n", lineMidpointPlanes[1].x);
	//~ printf("\tB: %f\n", lineMidpointPlanes[1].y);
	//~ printf("\tC: %f\n\n", lineMidpointPlanes[1].z);
	//~ 
	//~ printf("Plane 14 Coefficients:\n");
	//~ printf("\tA: %f\n", lineMidpointPlanes[2].x);
	//~ printf("\tB: %f\n", lineMidpointPlanes[2].y);
	//~ printf("\tC: %f\n\n", lineMidpointPlanes[2].z);
	//~ 
	//~ printf("Plane 23 Coefficients:\n");
	//~ printf("\tA: %f\n", lineMidpointPlanes[3].x);
	//~ printf("\tB: %f\n", lineMidpointPlanes[3].y);
	//~ printf("\tC: %f\n\n", lineMidpointPlanes[3].z);
	
	findIntersectionPoint(lineMidpointPlanes[0], lineMidpointPlanes[3], tempPt1, tempPt2);
	
	//~ printf("Point 1:\n");
    //~ printf("x: %f\n", 	tempPt1.x);
    //~ printf("y: %f\n", 	tempPt1.y);
    //~ printf("z: %f\n\n", tempPt1.z);
    //~ 
	//~ printf("Point 2:\n");
    //~ printf("x: %f\n", 	tempPt2.x);
    //~ printf("y: %f\n", 	tempPt2.y);
    //~ printf("z: %f\n\n", tempPt2.z);
    
    tempPt2.x = tempPt1.x + 100.0*tempPt2.x;
    tempPt2.y = tempPt1.y + 100.0*tempPt2.y;
    tempPt2.z = tempPt1.z + 100.0*tempPt2.z;
    //~ 
    //~ 
    //~ 
	//~ printf("Point 2...AGAIN!:\n");
    //~ printf("x: %f\n", 	tempPt2.x);
    //~ printf("y: %f\n", 	tempPt2.y);
    //~ printf("z: %f\n\n", tempPt2.z);
    
    //~ d = 
    
	u = ((lineMidpointPlanes[2].x*tempPt1.x)+(lineMidpointPlanes[2].y*tempPt1.y)-tempPt1.z+lineMidpointPlanes[2].z) /
		((lineMidpointPlanes[2].x*(tempPt1.x-tempPt2.x))+(lineMidpointPlanes[2].y*(tempPt1.y-tempPt2.y))-(tempPt1.z-tempPt2.z));
	
	//~ cout << u << endl;
	
	//~ centroidPt.x = 5*(1.0-d) + d*2;
	//~ centroidPt.y = 0*(1.0-d) + d*0;
	//~ centroidPt.z = 0*(1.0-d) + d*0;
	
	centroidPt.x = /*(int)*/(tempPt1.x*(1.0-u) + u*tempPt2.x);
	centroidPt.y = /*(int)*/(tempPt1.y*(1.0-u) + u*tempPt2.y);
	centroidPt.z = /*(int)*/(tempPt1.z*(1.0-u) + u*tempPt2.z);
	
	//~ printf("Centroid:\n");
    //~ printf("x: %f\n", 	centroidPt.x);
    //~ printf("y: %f\n", 	centroidPt.y);
    //~ printf("z: %f\n\n", centroidPt.z);
    
    possCentroids[index].centroid.x = centroidPt.x;
    possCentroids[index].centroid.y = centroidPt.y;
    possCentroids[index].centroid.z = centroidPt.z;
    possCentroids[index].votes++;
    
    //~ 
	//~ printf("Point 2:\n");
    //~ printf("x: %f\n", edge_pxls[pxls[1]].x);
    //~ printf("y: %f\n", edge_pxls[pxls[1]].y);
    //~ printf("z: %f\n\n", edge_pxls[pxls[1]].z);
    //~ 
	//~ printf("Midpoint:\n");
    //~ printf("x: %f\n", midPoints[0].x);
    //~ printf("y: %f\n", midPoints[0].y);
    //~ printf("z: %f\n\n", midPoints[0].z);
}
	
//~ void findMeanCentroid(accum_element_centroid accum_val, point_3d_t & meanCentroid)
//~ {
	//~ double sum_x = 0, sum_y = 0, sum_z = 0;
	
	//~ for(int i = 0; i < accum_val.votes; i++)
	//~ {
		//~ sum_x = accum_val.pts.cells[i][0].x;
		//~ sum_y = accum_val.pts.cells[i][0].y;
		//~ sum_z = accum_val.pts.cells[i][0].z;
	//~ }
	
	//~ meanCentroid.x = sum_x / (double)accum_val.votes;
	//~ meanCentroid.y = sum_y / (double)accum_val.votes;
	//~ meanCentroid.z = sum_z / (double)accum_val.votes;
//~ }

void findMidPoint(point_3d_t point1, point_3d_t point2, point_3d_t & midPoint)
{
	midPoint.x = (point2.x + point1.x) / 2.0;
	midPoint.y = (point2.y + point1.y) / 2.0;
	midPoint.z = (point2.z + point1.z) / 2.0;
}

void getLineMidpointPlanes(point_3d_t midPoint, plane_coeff & plane, int i)
{
	// This works...I think
	point_3d_t B, C, AB, AC, N;
	B.x = intersectionLines.cells[i][0].x + 0.3*intersectionLines.cells[i][1].x;
	B.y = intersectionLines.cells[i][0].y + 0.3*intersectionLines.cells[i][1].y;
	B.z = intersectionLines.cells[i][0].z + 0.3*intersectionLines.cells[i][1].z;
	
	C.x = intersectionLines.cells[i][0].x + 0.6*intersectionLines.cells[i][1].x;
	C.y = intersectionLines.cells[i][0].y + 0.6*intersectionLines.cells[i][1].y;
	C.z = intersectionLines.cells[i][0].z + 0.6*intersectionLines.cells[i][1].z;
	//~ 
	//~ AB.x = B.x - midPoint.x;
	//~ AB.y = B.y - midPoint.y;
	//~ AB.z = B.z - midPoint.z;
	//~ 
	//~ AC.x = C.x - midPoint.x;
	//~ AC.y = C.y - midPoint.y;
	//~ AC.z = C.z - midPoint.z;
	//~ 
	//~ crossProduct(AB, AC, plane);
	
	//~ printf("Normal:\n");
    //~ printf("x: %f\n", 	plane.x);
    //~ printf("y: %f\n", 	plane.y);
    //~ printf("z: %f\n\n", plane.z);

	
	//~ int i,j;
	double *results;
	//~ //pixel_distance *closest = new pixel_distance[7];
	double sum_xx = 0.0, sum_yy = 0.0, sum_xy = 0.0,
		   sum_x = 0.0, sum_y = 0.0, sum_xz = 0.0, sum_yz = 0.0, sum_z = 0.0;
	//~ 
	//~ // allocate
	matrix_allocate(&aug, 3, 4);
	
	results = new double[3];
	memset(results,0,sizeof(double) * 3);
	//~ 
	//~ //findNeighborhood(edge_pxls[pxl], closest);
	//~ 
	//~ //for(int i = 0; i < 6; i++)
	//~ //{
		sum_xx += B.x * B.x;
		sum_yy += B.y * B.y;
		sum_xy += B.x * B.y;
		sum_xz += B.x * B.z;
		sum_yz += B.y * B.z;
		sum_x  += B.x;
		sum_y  += B.y;
		sum_z  += B.z;
	//}
	
		sum_xx += C.x * C.x;
		sum_yy += C.y * C.y;
		sum_xy += C.x * C.y;
		sum_xz += C.x * C.z;
		sum_yz += C.y * C.z;
		sum_x  += C.x;
		sum_y  += C.y;
		sum_z  += C.z;
		//~ 
		//~ sum_xx += intersectionLines.cells[i][0].x * intersectionLines.cells[i][0].x;
		//~ sum_yy += intersectionLines.cells[i][0].y * intersectionLines.cells[i][0].y;
		//~ sum_xy += intersectionLines.cells[i][0].x * intersectionLines.cells[i][0].y;
		//~ sum_xz += intersectionLines.cells[i][0].x * intersectionLines.cells[i][0].z;
		//~ sum_yz += intersectionLines.cells[i][0].y * intersectionLines.cells[i][0].z;
		//~ sum_x  += intersectionLines.cells[i][0].x;
		//~ sum_y  += intersectionLines.cells[i][0].y;
		//~ sum_z  += intersectionLines.cells[i][0].z;
	//~ 
		//~ sum_xx += intersectionLines.cells[i][1].x * intersectionLines.cells[i][1].x;
		//~ sum_yy += intersectionLines.cells[i][1].y * intersectionLines.cells[i][1].y;
		//~ sum_xy += intersectionLines.cells[i][1].x * intersectionLines.cells[i][1].y;
		//~ sum_xz += intersectionLines.cells[i][1].x * intersectionLines.cells[i][1].z;
		//~ sum_yz += intersectionLines.cells[i][1].y * intersectionLines.cells[i][1].z;
		//~ sum_x  += intersectionLines.cells[i][1].x;
		//~ sum_y  += intersectionLines.cells[i][1].y;
		//~ sum_z  += intersectionLines.cells[i][1].z;
		
		sum_xx += midPoint.x * midPoint.x;
		sum_yy += midPoint.y * midPoint.y;
		sum_xy += midPoint.x * midPoint.y;
		sum_xz += midPoint.x * midPoint.z;
		sum_yz += midPoint.y * midPoint.z;
		sum_x  += midPoint.x;
		sum_y  += midPoint.y;
		sum_z  += midPoint.z;
		
	aug.cells[0][0] = sum_xx;
	aug.cells[0][1] = sum_xy;
	aug.cells[1][0] = sum_xy;
	aug.cells[0][2] = sum_x;
	aug.cells[2][0] = sum_x;
	aug.cells[1][1] = sum_yy;
	aug.cells[1][2] = sum_y;
	aug.cells[2][1] = sum_y;
	aug.cells[2][2] = 3;
	aug.cells[0][3] = sum_xz;
	aug.cells[1][3] = sum_yz;
	aug.cells[2][3] = sum_z;

	// run a gaussian solver on the system of equations
	//~ printf("==========================================\n");
	//~ matrix_print("Augmented (pre-gauss)",&aug);
	gauss(&aug,results);
	//~ matrix_print("Augmented (post-gauss)",&aug);
	//~ printf("==========================================\n");

	// extract the results
	plane.x = results[0];
	plane.y = results[1];
	plane.z = results[2];
	
	//~ printf("A: %f\n", plane.x);
	//~ printf("B: %f\n", plane.y);
	//~ printf("C: %f\n\n", plane.z);

	// cleanup
	delete [] results;
}

void insertionSort(pixel_distance arr[],int length)
{
	
	int i, j;
	pixel_distance tmp;
	for(i = 1; i < length; i++)
	{
		j = i;
		while( j > 0 && arr[j-1].distance > arr[j].distance)
		{
			tmp = arr[j];
			arr[j] = arr[j-1];
			arr[j-1] = tmp;
			j--;
		}
	}
}

void crossProduct(point_3d_t vec1, point_3d_t vec2, point_3d_t & crossProd)
{
	crossProd.x = vec1.y*vec2.z - vec2.y*vec1.z;
	crossProd.y = vec1.z*vec2.x - vec2.z*vec1.x;
	crossProd.z = vec1.x*vec2.y - vec2.x*vec1.y;
}

double dotProduct(point_3d_t vec1, point_3d_t vec2)
{
	return vec1.x*vec2.x + vec1.y*vec2.y + vec1.z*vec2.z;
}
